
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       blog.c
  * @author     baiyang
  * @date       2021-8-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <string.h>

#include <rtthread.h>

#include "blog.h"
#include "blog_interface.h"

#include <common/time/gp_time.h>
#include <parameter/param.h>
#include <common/console/console.h>
#include <file_manager/file_manager.h>
/*-----------------------------------macro------------------------------------*/
#ifndef OS_ENTER_CRITICAL
    #define OS_ENTER_CRITICAL        rt_enter_critical()
#endif

#ifndef OS_EXIT_CRITICAL
    #define OS_EXIT_CRITICAL         rt_exit_critical()
#endif

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static char* blog_tag = "[blog]";

/* BLog element define */
blog_elem_t IMU_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("gyr_x", BLOG_FLOAT),
    BLOG_ELEMENT("gyr_y", BLOG_FLOAT),
    BLOG_ELEMENT("gyr_z", BLOG_FLOAT),
    BLOG_ELEMENT("acc_x", BLOG_FLOAT),
    BLOG_ELEMENT("acc_y", BLOG_FLOAT),
    BLOG_ELEMENT("acc_z", BLOG_FLOAT),
    BLOG_ELEMENT("fil_gyr_x", BLOG_FLOAT),
    BLOG_ELEMENT("fil_gyr_y", BLOG_FLOAT),
    BLOG_ELEMENT("fil_gyr_z", BLOG_FLOAT),
    BLOG_ELEMENT("fil_acc_x", BLOG_FLOAT),
    BLOG_ELEMENT("fil_acc_y", BLOG_FLOAT),
    BLOG_ELEMENT("fil_acc_z", BLOG_FLOAT),
};

blog_elem_t MAG_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("mag_x", BLOG_FLOAT),
    BLOG_ELEMENT("mag_y", BLOG_FLOAT),
    BLOG_ELEMENT("mag_z", BLOG_FLOAT),
};

blog_elem_t MAG_FIL_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("mag_x", BLOG_FLOAT),
    BLOG_ELEMENT("mag_y", BLOG_FLOAT),
    BLOG_ELEMENT("mag_z", BLOG_FLOAT),
};

blog_elem_t MAG_MEA_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("mag_x", BLOG_FLOAT),
    BLOG_ELEMENT("mag_y", BLOG_FLOAT),
    BLOG_ELEMENT("mag_z", BLOG_FLOAT),
};

blog_elem_t Barometer_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("pressure", BLOG_FLOAT),
    BLOG_ELEMENT("temperature", BLOG_FLOAT),
};

blog_elem_t GPS_uBlox_Elems[] = {
    BLOG_ELEMENT("timestamp",   BLOG_UINT32),
    BLOG_ELEMENT("timegpsms",   BLOG_UINT32),
    BLOG_ELEMENT("fixType", BLOG_UINT8),
    BLOG_ELEMENT("numSV",   BLOG_UINT8),
    BLOG_ELEMENT("lon",     BLOG_INT32),
    BLOG_ELEMENT("lat",     BLOG_INT32),
    BLOG_ELEMENT("height",  BLOG_INT32),
    BLOG_ELEMENT("hAcc",    BLOG_FLOAT),
    BLOG_ELEMENT("vAcc",    BLOG_FLOAT),
    BLOG_ELEMENT("sAcc",    BLOG_FLOAT),
    BLOG_ELEMENT("velN",    BLOG_FLOAT),
    BLOG_ELEMENT("velE",    BLOG_FLOAT),
    BLOG_ELEMENT("velD",    BLOG_FLOAT),
    BLOG_ELEMENT("speed",   BLOG_FLOAT),
    BLOG_ELEMENT("headMot", BLOG_FLOAT),
    BLOG_ELEMENT("headAcc", BLOG_FLOAT),
};

blog_elem_t ATT_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("roll", BLOG_FLOAT),
    BLOG_ELEMENT("pitch", BLOG_FLOAT),
    BLOG_ELEMENT("yaw", BLOG_FLOAT),
    BLOG_ELEMENT("RollRate", BLOG_FLOAT),
    BLOG_ELEMENT("PitchRate", BLOG_FLOAT),
    BLOG_ELEMENT("YawRate", BLOG_FLOAT),
};

blog_elem_t POS_XY_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("Px", BLOG_FLOAT),
    BLOG_ELEMENT("Py", BLOG_FLOAT),
    BLOG_ELEMENT("Vx", BLOG_FLOAT),
    BLOG_ELEMENT("Vy", BLOG_FLOAT),
    BLOG_ELEMENT("Ax", BLOG_FLOAT),
    BLOG_ELEMENT("Ay", BLOG_FLOAT),
};

blog_elem_t ATC_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("DesRoll", BLOG_FLOAT),
    BLOG_ELEMENT("DesPitch", BLOG_FLOAT),
    BLOG_ELEMENT("DesYaw", BLOG_FLOAT),
    BLOG_ELEMENT("DesRollRate", BLOG_FLOAT),
    BLOG_ELEMENT("DesPitchRate", BLOG_FLOAT),
    BLOG_ELEMENT("DesYawRate", BLOG_FLOAT),
};

blog_elem_t PSC_XY_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("DesPx", BLOG_FLOAT),
    BLOG_ELEMENT("DesPy", BLOG_FLOAT),
    BLOG_ELEMENT("DesVx", BLOG_FLOAT),
    BLOG_ELEMENT("DesVy", BLOG_FLOAT),
    BLOG_ELEMENT("DesAx", BLOG_FLOAT),
    BLOG_ELEMENT("DesAy", BLOG_FLOAT),
};

blog_elem_t POS_Z_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("Pz", BLOG_FLOAT),
    BLOG_ELEMENT("Vz", BLOG_FLOAT),
    BLOG_ELEMENT("Az", BLOG_FLOAT),
};

blog_elem_t PSC_Z_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("DesPz", BLOG_FLOAT),
    BLOG_ELEMENT("DesVz", BLOG_FLOAT),
    BLOG_ELEMENT("DesAz", BLOG_FLOAT),
};

blog_elem_t RC_IN_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("rc1", BLOG_UINT16),
    BLOG_ELEMENT("rc2", BLOG_UINT16),
    BLOG_ELEMENT("rc3", BLOG_UINT16),
    BLOG_ELEMENT("rc4", BLOG_UINT16),
    BLOG_ELEMENT("rc5", BLOG_UINT16),
    BLOG_ELEMENT("rc6", BLOG_UINT16),
    BLOG_ELEMENT("rc7", BLOG_UINT16),
    BLOG_ELEMENT("rc8", BLOG_UINT16),
    BLOG_ELEMENT("rc9", BLOG_UINT16),
    BLOG_ELEMENT("rc10", BLOG_UINT16),
    BLOG_ELEMENT("rc11", BLOG_UINT16),
    BLOG_ELEMENT("rc12", BLOG_UINT16),
    BLOG_ELEMENT("rc13", BLOG_UINT16),
    BLOG_ELEMENT("rc14", BLOG_UINT16),
    BLOG_ELEMENT("rc15", BLOG_UINT16),
    BLOG_ELEMENT("rc16", BLOG_UINT16),
    BLOG_ELEMENT("rc17", BLOG_UINT16),
    BLOG_ELEMENT("rc18", BLOG_UINT16),
};

blog_elem_t SRV_OUT_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("srv1", BLOG_UINT16),
    BLOG_ELEMENT("srv2", BLOG_UINT16),
    BLOG_ELEMENT("srv3", BLOG_UINT16),
    BLOG_ELEMENT("srv4", BLOG_UINT16),
    BLOG_ELEMENT("srv5", BLOG_UINT16),
    BLOG_ELEMENT("srv6", BLOG_UINT16),
    BLOG_ELEMENT("srv7", BLOG_UINT16),
    BLOG_ELEMENT("srv8", BLOG_UINT16),
};

blog_elem_t VER_CTRL_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("ThrIn",     BLOG_FLOAT),
    BLOG_ELEMENT("DesAlt",    BLOG_FLOAT),
    BLOG_ELEMENT("InavAlt",   BLOG_FLOAT),
    BLOG_ELEMENT("BaroAlt",   BLOG_FLOAT),
    BLOG_ELEMENT("DesCR",     BLOG_FLOAT),
    BLOG_ELEMENT("CR",        BLOG_FLOAT),
};

blog_elem_t MODE_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("mode",      BLOG_UINT8),
    BLOG_ELEMENT("ModeReason",BLOG_UINT8),
};

blog_elem_t EVENT_Elems[] = {
    BLOG_ELEMENT("timestamp", BLOG_UINT32),
    BLOG_ELEMENT("event",      BLOG_UINT8),
    BLOG_ELEMENT("EventCode",  BLOG_INT8),
};

blog_elem_t PID_Elems[] = {
    BLOG_ELEMENT("timestamp",   BLOG_UINT32),
    BLOG_ELEMENT("RollRatTar",  BLOG_FLOAT),
    BLOG_ELEMENT("RollRatErr",  BLOG_FLOAT),
    BLOG_ELEMENT("PitchRatTar", BLOG_FLOAT),
    BLOG_ELEMENT("PitchRatErr", BLOG_FLOAT),
    BLOG_ELEMENT("YawRatTar",   BLOG_FLOAT),
    BLOG_ELEMENT("YawRatErr",   BLOG_FLOAT),
};

blog_elem_t MISSION_Elems[] = {
    BLOG_ELEMENT("timestamp",   BLOG_UINT32),
    BLOG_ELEMENT("CTot",        BLOG_UINT16),
    BLOG_ELEMENT("Seq",         BLOG_UINT16),
    BLOG_ELEMENT("Cmd",         BLOG_UINT16),
    BLOG_ELEMENT("Prm1",        BLOG_FLOAT),
    BLOG_ELEMENT("Prm2",        BLOG_FLOAT),
    BLOG_ELEMENT("Prm3",        BLOG_FLOAT),
    BLOG_ELEMENT("Prm4",        BLOG_FLOAT),
    BLOG_ELEMENT("Lat",         BLOG_UINT32),
    BLOG_ELEMENT("Lng",         BLOG_UINT32),
    BLOG_ELEMENT("Alt",         BLOG_FLOAT),
    BLOG_ELEMENT("Frame",       BLOG_UINT8),
};

blog_elem_t LOITER_Elems[] = {
    BLOG_ELEMENT("timestamp",   BLOG_UINT32),
    BLOG_ELEMENT("daccx",       BLOG_FLOAT),
    BLOG_ELEMENT("daccy",       BLOG_FLOAT),
    BLOG_ELEMENT("paccx",       BLOG_FLOAT),
    BLOG_ELEMENT("paccy",       BLOG_FLOAT),
    BLOG_ELEMENT("bacc",        BLOG_FLOAT),
    BLOG_ELEMENT("laccbx",      BLOG_FLOAT),
    BLOG_ELEMENT("laccby",      BLOG_FLOAT),
    BLOG_ELEMENT("dvelx",       BLOG_FLOAT),
    BLOG_ELEMENT("dvely",       BLOG_FLOAT),
    BLOG_ELEMENT("dspeed",      BLOG_FLOAT),
    BLOG_ELEMENT("decel",       BLOG_FLOAT),
    BLOG_ELEMENT("dvnx",        BLOG_FLOAT),
    BLOG_ELEMENT("dvny",        BLOG_FLOAT),
};

/* BLog bus define */
blog_bus_t _blog_bus[] = {
    BLOG_BUS("IMU",       BLOG_IMU_ID,      IMU_Elems),
    BLOG_BUS("MAG",       BLOG_MAG_ID,      MAG_Elems),
    BLOG_BUS("MAG_Fil",   BLOG_MAG_FIL_ID,  MAG_FIL_Elems),
    BLOG_BUS("MAG_Mea",   BLOG_MAG_MEA_ID,  MAG_MEA_Elems),
    BLOG_BUS("Barometer", BLOG_BARO_ID,     Barometer_Elems),
    BLOG_BUS("GPS_uBlox", BLOG_GPS_ID,      GPS_uBlox_Elems),
    BLOG_BUS("ATT",       BLOG_ATT_ID,      ATT_Elems),
    BLOG_BUS("ATC",       BLOG_ATC_ID,      ATC_Elems),
    BLOG_BUS("POS_XY",    BLOG_POS_XY_ID,   POS_XY_Elems),
    BLOG_BUS("POS_Z",     BLOG_POS_Z_ID,    POS_Z_Elems),
    BLOG_BUS("PSC_XY",    BLOG_PSC_XY_ID,   PSC_XY_Elems),
    BLOG_BUS("PSC_Z",     BLOG_PSC_Z_ID,    PSC_Z_Elems),
    BLOG_BUS("RC_IN",     BLOG_RC_IN_ID,    RC_IN_Elems),
    BLOG_BUS("SRV",       BLOG_SRV_OUT_ID,  SRV_OUT_Elems),
    BLOG_BUS("VCTRL",     BLOG_VER_CTRL_ID, VER_CTRL_Elems),
    BLOG_BUS("MODE",      BLOG_MODE_ID,     MODE_Elems),
    BLOG_BUS("EVENT",     BLOG_EVENT_ID,    EVENT_Elems),
    BLOG_BUS("PID",       BLOG_PID_ID,      PID_Elems),
    BLOG_BUS("MISSION",   BLOG_MISSION_CMD_ID, MISSION_Elems),
    BLOG_BUS("LOITER",    BLOG_LOITER_ID,   LOITER_Elems),
};

typedef struct {
    uint32_t total_msg;
    uint32_t lost_msg;
} blog_stat_t;

struct rtp_blog {
    int fid;
    uint8_t file_open;
    char file_name[50];
    uint8_t log_status;
    blog_header_t header;
    blog_buffer_t buffer;
    blog_stat_t monitor[sizeof(_blog_bus) / sizeof(blog_bus_t)];
};

static struct rtp_blog blog = { 0 };

typedef struct {
  uint32_t period;
  int8_t model_info[16];
} struct_TYt7YeNdxIDXfczXumtXXB;

typedef struct {
  uint32_t period;
  int8_t model_info[23];
} struct_biZzOMrg0u3lxrb7POOubF;

struct_TYt7YeNdxIDXfczXumtXXB INS_EXPORT = {
  2U,

  { 66, 97, 115, 101, 32, 73, 78, 83, 32, 118, 48, 46, 49, 46, 49, 0 }
} ;

struct_TYt7YeNdxIDXfczXumtXXB FMS_EXPORT = {
  4U,

  { 66, 97, 115, 101, 32, 70, 77, 83, 32, 118, 48, 46, 49, 46, 50, 0 }
} ;

struct_biZzOMrg0u3lxrb7POOubF CONTROL_EXPORT = {
  2U,

  { 66, 97, 115, 101, 32, 67, 111, 110, 116, 114, 111, 108, 108, 101, 114, 32,
    118, 48, 46, 49, 46, 49, 0 }
} ;

static void (*_blog_start_cb[BLOG_MAX_CALLBACK_NUM])(void);
static void (*_blog_stop_cb[BLOG_MAX_CALLBACK_NUM])(void);
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void _reset_log_buffer(void)
{
    blog.buffer.num_sector = BLOG_BUFFER_SIZE / BLOG_SECTOR_SIZE;
    blog.buffer.head = 0;
    blog.buffer.tail = 0;
    blog.buffer.index = 0;
}

static int32_t _get_bus_index(uint8_t msg_id)
{
    for (int i = 0; i < sizeof(_blog_bus) / sizeof(blog_bus_t); i++) {
        if (_blog_bus[i].msg_id == msg_id) {
            return i;
        }
    }

    return -1;
}

static int _file_write(const void* payload, uint16_t len)
{
    int bw;

    if (!blog.file_open) {
        /* no log file is opened */
        return 0;
    }

    bw = write(blog.fid, payload, len);

    return bw;
}

static uint16_t _get_max_write_sector(uint32_t head_p, uint32_t tail_p)
{
    uint16_t sector_to_end;
    uint16_t sector_in_buffer;
    uint16_t sector_to_write;

    sector_to_end = blog.buffer.num_sector - tail_p;

    if (head_p >= tail_p) {
        sector_in_buffer = head_p - tail_p;
    } else {
        sector_in_buffer = sector_to_end + head_p;
    }

    sector_to_write = sector_in_buffer < sector_to_end ? sector_in_buffer : sector_to_end;

    return sector_to_write <= BLOG_MAX_SECTOR_TO_WRITE ? sector_to_write : BLOG_MAX_SECTOR_TO_WRITE;
}

static bool _buffer_check_full(uint32_t len_to_write)
{
    uint32_t free_space_in_sector = BLOG_SECTOR_SIZE - blog.buffer.index;

    // TODO: check if write multiple sectors at once

    /* check if buffer is full */
    if (free_space_in_sector < len_to_write) {
        if ((blog.buffer.head + 1) % blog.buffer.num_sector == blog.buffer.tail) {
            return true;
        }
    }

    return false;
}

static void _buffer_putc(uint8_t ch)
{
    uint32_t free_space_in_sector = BLOG_SECTOR_SIZE - blog.buffer.index;

    if (free_space_in_sector < 1) {
        // move head point to next sector
        blog.buffer.head = (blog.buffer.head + 1) % blog.buffer.num_sector;
        blog.buffer.index = 0;

        //logger_send_event(EVENT_BLOG_UPDATE);
    }

    blog.buffer.data[blog.buffer.head * BLOG_SECTOR_SIZE + blog.buffer.index] = ch;
    blog.buffer.index += 1;
}

static void _buffer_write(const uint8_t* data, uint16_t len)
{
    uint32_t free_space_in_sector = BLOG_SECTOR_SIZE - blog.buffer.index;

    // TODO: add support with len larger than BLOG_SECTOR_SIZE

    if (free_space_in_sector < len) {
        memcpy(&blog.buffer.data[blog.buffer.head * BLOG_SECTOR_SIZE + blog.buffer.index], data, free_space_in_sector);

        // move head point to next sector
        blog.buffer.head = (blog.buffer.head + 1) % blog.buffer.num_sector;
        blog.buffer.index = 0;

        memcpy(&blog.buffer.data[blog.buffer.head * BLOG_SECTOR_SIZE + blog.buffer.index], &data[free_space_in_sector], len - free_space_in_sector);
        blog.buffer.index += len - free_space_in_sector;

        /* we have a new sector data, send blog update event to wakeup logger thread */
        //logger_send_event(EVENT_BLOG_UPDATE);
    } else {
        memcpy(&blog.buffer.data[blog.buffer.head * BLOG_SECTOR_SIZE + blog.buffer.index], data, len);
        blog.buffer.index += len;
    }
}

static void _invoke_callback_func(uint8_t cb_type)
{
    uint32_t i;

    if (cb_type == BLOG_CB_START) {
        for (i = 0; i < BLOG_MAX_CALLBACK_NUM; i++) {
            if (_blog_start_cb[i]) {
                _blog_start_cb[i]();
            }
        }
    }

    if (cb_type == BLOG_CB_STOP) {
        for (i = 0; i < BLOG_MAX_CALLBACK_NUM; i++) {
            if (_blog_stop_cb[i]) {
                _blog_stop_cb[i]();
            }
        }
    }
}

gp_err blog_push_msg_data(const void* payload, uint16_t len)
{
    int32_t bus_index;

    /* chceck log status */
    if (blog.log_status != BLOG_STATUS_LOGGING) {
        return GP_EEMPTY;
    }

    blog_take_mutex_block();

    /* check if buffer has enough space to store data */
    if (_buffer_check_full(len)) {
        TIMETAG_CHECK_EXECUTE(blog_buff_full1, 500, console_printf("%s buffer is full\n",blog_tag);)
        blog_release_mutex();
        return GP_EFULL;
    }

    /* write payload */
    _buffer_write(payload, len);

    bus_index = _get_bus_index(((uint8_t*)payload)[2]);

    if (bus_index >= 0) {
        blog.monitor[bus_index].total_msg += 1;
    }

    blog_release_mutex();
    return GP_EOK;
}

gp_err blog_add_desc(char* desc)
{
    if (strlen(desc) > BLOG_DESCRIPTION_SIZE - 1) {
        console_printf("%s description too long.\n",blog_tag);
        return GP_ENOMEM;
    }

    strcpy(blog.header.description, desc);

    return GP_EOK;
}

gp_err blog_start(char* file_name)
{
    if (blog.log_status != BLOG_STATUS_IDLE) {
        if (PARAM_GET_INT32(VEHICLE, BLOG_MODE) != 1) {
            console_printf("%s %s is logging, stop it first\n", blog_tag, blog.file_name);
        }
        return GP_EBUSY;
    }

    /*********************** create log file ***********************/
    blog.fid = open(file_name, O_CREAT | O_WRONLY);

    if (blog.fid < 0) {
        console_printf("%s %s open fail\n", blog_tag, file_name);
        return GP_ERROR;
    }

    /* set log file open flag */
    blog.file_open = 1;

    blog.header.timestamp = time_millis();

    /*********************** init log buffer ***********************/
    blog.buffer.head = blog.buffer.tail = 0;
    blog.buffer.index = 0;

    /*********************** write log header ***********************/
    blog.log_status = BLOG_STATUS_WRITE_HEAD;

    /* TODO: maybe we need check if write success */

    /* write log info */
    _file_write(&blog.header.version, sizeof(blog.header.version));
    _file_write(&blog.header.timestamp, sizeof(blog.header.timestamp));
    _file_write(&blog.header.max_name_len, sizeof(blog.header.max_name_len));
    _file_write(&blog.header.max_desc_len, sizeof(blog.header.max_desc_len));
    _file_write(&blog.header.max_model_info_len, sizeof(blog.header.max_model_info_len));
    _file_write(blog.header.description, BLOG_DESCRIPTION_SIZE);

    sprintf(blog.header.model_info, "%s\n%s\n%s", (char*)INS_EXPORT.model_info, (char*)FMS_EXPORT.model_info,
        (char*)CONTROL_EXPORT.model_info);

    _file_write(blog.header.model_info, BLOG_MODEL_INFO_SIZE);
    // clear description after it has been written
    memset(blog.header.description, 0, BLOG_DESCRIPTION_SIZE);

    /* write bus info */
    _file_write(&blog.header.num_bus, sizeof(blog.header.num_bus));

    for (int n = 0; n < blog.header.num_bus; n++) {
        _file_write(blog.header.bus_list[n].name, BLOG_MAX_NAME_LEN);
        _file_write(&blog.header.bus_list[n].msg_id, sizeof(blog.header.bus_list[n].msg_id));
        _file_write(&blog.header.bus_list[n].num_elem, sizeof(blog.header.bus_list[n].num_elem));

        // write bus element
        for (int k = 0; k < blog.header.bus_list[n].num_elem; k++) {
            _file_write(blog.header.bus_list[n].elem_list[k].name, BLOG_MAX_NAME_LEN);
            _file_write(&blog.header.bus_list[n].elem_list[k].type, sizeof(blog.header.bus_list[n].elem_list[k].type));
            _file_write(&blog.header.bus_list[n].elem_list[k].number, sizeof(blog.header.bus_list[n].elem_list[k].number));
        }
    }

    /*********************** write parameter info ***********************/
    _file_write(&blog.header.num_param_group, sizeof(blog.header.num_param_group));

    char name_buffer[BLOG_MAX_NAME_LEN + 1];

    for (int n = 0; n < blog.header.num_param_group; n++) {
        memset(name_buffer, 0, BLOG_MAX_NAME_LEN);
        strncpy(name_buffer, blog.header.param_group_list[n].name, BLOG_MAX_NAME_LEN);

        _file_write(name_buffer, BLOG_MAX_NAME_LEN);
        _file_write(&blog.header.param_group_list[n].param_num, sizeof(blog.header.param_group_list[n].param_num));

        for (uint32_t k = 0; k < blog.header.param_group_list[n].param_num; k++) {
            memset(name_buffer, 0, BLOG_MAX_NAME_LEN);
            strncpy(name_buffer, blog.header.param_group_list[n].content[k].name, BLOG_MAX_NAME_LEN);

            _file_write(name_buffer, BLOG_MAX_NAME_LEN);
            _file_write(&blog.header.param_group_list[n].content[k].type, sizeof(blog.header.param_group_list[n].content[k].type));

            int type = blog.header.param_group_list[n].content[k].type;

            if (type == PARAM_TYPE_INT8) {
                _file_write(&blog.header.param_group_list[n].content[k].val.i8, sizeof(int8_t));
            } else if (type == PARAM_TYPE_UINT8) {
                _file_write(&blog.header.param_group_list[n].content[k].val.u8, sizeof(uint8_t));
            } else if (type == PARAM_TYPE_INT16) {
                _file_write(&blog.header.param_group_list[n].content[k].val.i16, sizeof(int16_t));
            } else if (type == PARAM_TYPE_UINT16) {
                _file_write(&blog.header.param_group_list[n].content[k].val.u16, sizeof(uint16_t));
            } else if (type == PARAM_TYPE_INT32) {
                _file_write(&blog.header.param_group_list[n].content[k].val.i32, sizeof(int32_t));
            } else if (type == PARAM_TYPE_UINT32) {
                _file_write(&blog.header.param_group_list[n].content[k].val.u32, sizeof(uint32_t));
            } else if (type == PARAM_TYPE_FLOAT) {
                _file_write(&blog.header.param_group_list[n].content[k].val.f, sizeof(float));
            } else if (type == PARAM_TYPE_DOUBLE) {
                _file_write(&blog.header.param_group_list[n].content[k].val.lf, sizeof(double));
            } else {
                console_printf("%s unknown parameter type:%d\n", blog_tag, type);
            }
        }
    }

    /*********************** set log status ***********************/
    strncpy(blog.file_name, file_name, sizeof(blog.file_name) - 1);

    for (int i = 0; i < sizeof(_blog_bus) / sizeof(blog_bus_t); i++) {
        blog.monitor[i].total_msg = 0;
        blog.monitor[i].lost_msg = 0;
    }

    // invoke callback function
    _invoke_callback_func(BLOG_CB_START);

    fsync(blog.fid);

    // start logging, set flag
    blog.log_status = BLOG_STATUS_LOGGING;

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    console_printf("%s start logging:%s\n", blog_tag, file_name);
#endif

    return GP_EOK;
}

void blog_stop(void)
{
    if (blog.log_status == BLOG_STATUS_LOGGING) {
        // set log status to stopping, let logger thread write reamined data in
        // buffer, then stop logging.
        blog.log_status = BLOG_STATUS_STOPPING;
    }
}

void blog_async_output(void)
{
    uint32_t head_p, tail_p;
    uint8_t need_sync = 0;

    if (!blog.file_open) {
        // no log file is opened
        return;
    }

    head_p = blog.buffer.head;
    tail_p = blog.buffer.tail;

    need_sync = (head_p != tail_p);

    // write log buffer sector into storage device 
    while (head_p != tail_p) {
        uint16_t sector_to_write = _get_max_write_sector(head_p, tail_p);

        write(blog.fid, &blog.buffer.data[tail_p * BLOG_SECTOR_SIZE], sector_to_write * BLOG_SECTOR_SIZE);

        tail_p = (tail_p + sector_to_write) % blog.buffer.num_sector;

        blog.buffer.tail = tail_p;
    }

    if (need_sync) {
        fsync(blog.fid);
    }

    // if logging is off, we need to clean up buffer.
    if (blog.log_status == BLOG_STATUS_STOPPING) {
        /* write rest data in buffer */
        if (blog.buffer.index) {
            write(blog.fid, &blog.buffer.data[tail_p * BLOG_SECTOR_SIZE], blog.buffer.index);
            fsync(blog.fid);
        }

        if (blog.file_open) {
            close(blog.fid);
            blog.fid = -1;
            blog.file_open = 0;
        }

        // invoke callback function
        _invoke_callback_func(BLOG_CB_STOP);

        // set log status to idle
        blog.log_status = BLOG_STATUS_IDLE;

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        console_printf("%s stop logging:%s", blog_tag, blog.file_name);
        for (int i = 0; i < sizeof(_blog_bus) / sizeof(blog_bus_t); i++) {
            console_printf("%s %-20s id:%-3d record:%-8d lost:%-5d\n", blog_tag, _blog_bus[i].name, _blog_bus[i].msg_id,
                blog.monitor[i].total_msg, blog.monitor[i].lost_msg);
        }
#endif
    }
}

uint8_t blog_get_status(void)
{
    return blog.log_status;
}

char* blog_get_logging_file_name(void)
{
    return blog.file_name;
}

void blog_show_status(void)
{
    for (int i = 0; i < sizeof(_blog_bus) / sizeof(blog_bus_t); i++) {
        console_printf("%s %-20s id:%-3d record:%-8d lost:%-5d\n", blog_tag, _blog_bus[i].name, _blog_bus[i].msg_id,
            blog.monitor[i].total_msg, blog.monitor[i].lost_msg);
    }
}

gp_err blog_register_callback(uint8_t cb_type, void (*cb)(void))
{
    uint32_t i;

    if (cb == NULL) {
        return GP_ERROR;
    }

    if (cb_type == BLOG_CB_START) {
        for (i = 0; i < BLOG_MAX_CALLBACK_NUM; i++) {
            if (_blog_start_cb[i] == NULL) {
                _blog_start_cb[i] = cb;
                return GP_EOK;
            }
        }
    } else if (cb_type == BLOG_CB_STOP) {
        for (i = 0; i < BLOG_MAX_CALLBACK_NUM; i++) {
            if (_blog_stop_cb[i] == NULL) {
                _blog_stop_cb[i] = cb;
                return GP_EOK;
            }
        }
    } else {
        return GP_EINVAL;
    }

    return GP_ERROR;
}

void blog_init(void)
{
    uint32_t i;

    blog.file_open = 0;
    blog.log_status = BLOG_STATUS_IDLE;

    /* initialize log header */
    blog.header.version = BLOG_VERSION;
    blog.header.timestamp = 0;
    blog.header.max_name_len = BLOG_MAX_NAME_LEN;
    blog.header.max_desc_len = BLOG_DESCRIPTION_SIZE;
    blog.header.max_model_info_len = BLOG_MODEL_INFO_SIZE;
    memset(blog.header.description, 0, BLOG_DESCRIPTION_SIZE);

    blog.header.num_bus = sizeof(_blog_bus) / sizeof(blog_bus_t);
    blog.header.bus_list = _blog_bus;
    blog.header.num_param_group = sizeof(param_list) / sizeof(param_group_t);
    blog.header.param_group_list = (param_group_t*)&param_list;

    // initialize log buffer
    blog.buffer.data = (uint8_t*)rt_malloc(BLOG_BUFFER_SIZE);
    if (blog.buffer.data == NULL) {
        console_printf("%s blog buffer malloc fail\n", blog_tag);
    }

    _reset_log_buffer();

    for (i = 0; i < BLOG_MAX_CALLBACK_NUM; i++) {
        _blog_start_cb[i] = _blog_stop_cb[i] = NULL;
    }

    blog_init_mutex();
}

/*------------------------------------test------------------------------------*/


